void setup(){
  SerialPort.begin(57600);

  pinMode(8,INPUT);    // Rx Radio Input
  pinMode(5,OUTPUT);   // Red LED
  pinMode(6,OUTPUT);   // Blue LED
  pinMode(7,OUTPUT);   // Yellow LED

  SerialPort.print("Easy IMU Pilot v");
  SerialPort.println(SOFTWARE_VER);

  // PPM decoder initialization
  ServoDecode.begin();

  // ADC initialization
  Analog_Reference(EXTERNAL);
  Analog_Init();

  // Magnetometer initialization
#if MAGNETOMETER == 1 //if you have a magnetometer
  I2C_Init(); 
  Compass_Init();
#endif

  // Offset values for accels and gyros
  AN_OFFSET[0] = gyro_offset_roll;
  AN_OFFSET[1] = gyro_offset_pitch;
  AN_OFFSET[2] = gyro_offset_yaw;
  AN_OFFSET[3] = acc_offset_x;
  AN_OFFSET[4] = acc_offset_y;
  AN_OFFSET[5] = acc_offset_z;

  // Control gains
  PIDroll.Pgain = KP_ROLL;
  PIDroll.Igain = KI_ROLL;
  PIDroll.Dgain = KD_ROLL;
  PIDpitch.Pgain = KP_PITCH;
  PIDpitch.Igain = KI_PITCH;
  PIDpitch.Dgain = KD_PITCH;
  PIDyaw.Pgain = KP_YAW;
  PIDyaw.Igain = KI_YAW;
  PIDyaw.Dgain = KD_YAW;

  //Servo initialization
#if OUTPUT_SERVOTIMER2 == 1  // Use SerialTimer2 library
  servo0.attach(9);
  servo1.attach(10);
  servo2.attach(11);
  servo3.attach(12);
#endif

  SerialPort.println("Ready to fly");
  digitalWrite(7,HIGH); // Light yellow led
}




